#include <stdio.h>
#include <string.h>
#include <memory.h>


#include "include/obd_function.h"
#include "include/obd_uart.h"
#include "include/vehicle.h"
#include "include/sysutils.h"
#include "include/stn.h"


const char OBD_PROTOCL_NAME[SUPPORT_PROTOCOLS_NUM][40] = { 
												"Autiomatic",
												"SAE J1850 PWM",
												"SAE J1850 VPW",
												"ISO 9141-2 (5 baud init)",
												"ISO 14230-4 KWP (5 baud init)",
												"ISO 14230-4 KWP (fast init)",
												"ISO 15765-4 CAN (11 bit ID, 500 kbaud)",
												"ISO 15765-4 CAN (29 bit ID, 500 kbaud)",
												"ISO 15765-4 CAN (11 bit ID, 250 kbaud)",
												"ISO 15765-4 CAN (29 bit ID, 250 kbaud)",
												"SAE J1939 CAN (29 bit ID, 250* kbaud)"
												};

const char PROTOCOLNAME[SUPPORT_PROTOCOLS_NUM][15] = { 
											"Automatic      ",
											"SAE-J1850-PWM  ",
											"SAE-J1850-VPW  ",
											"ISO-9141-2     ",
											"KWP-5baud-init ",
											"KWP-fast-init  ",
											"CAN-11-500kbaud",
											"CAN-29-500kbaud",
											"CAN-11-250kbaud",
											"CAN-29-250kbaud",
											"SAE-J1939-CAN  "
											};

const int MODE01_PID_LEN[] = {
					4,	//0x00 PIDs supported[01 - 20]
					4,	//0x01 Monitor Status since DTCs cleared.
					2,	//0x02 Freeze DTC
					2,	//0x03 Fuel system status
					1,  //0x04 Calculated engine load value
					1,	//0x05 Engine coolant temperature
					1,	//0x06 Short term fuel % Bank 1
					1,	//0x07 Long term fuel % Bank 1
					1,	//0x08 Short term fuel % Bank 2
					1,	//0X09 Long term fuel % Bank 2
					1,	//0x0A Fuel pressure
					1,  //0x0B Intake manifold absolute pressure
					2,	//0x0C Engine RPM
					1,	//0x0D Vehicle speed
					1,	//0x0E Timing advance
					1,	//0x0F Intake air temperature
					2,	//0x10 MAF air flow rate
					1,	//0x11 Throttle position
					1,	//0x12 Commanded secondary air status
					1,	//0x13 Oxygen sensors present
					2,	//0x14 Bank 1, Sensor 1:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x15 Bank 1, Sensor 2:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x16 Bank 1, Sensor 3:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x17 Bank 1, Sensor 4:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x18 Bank 2, Sensor 1:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x19 Bank 2, Sensor 2:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x1A Bank 2, Sensor 3:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					2,	//0x1B Bank 2, Sensor 4:
						//		Oxygen sensor voltage,
						//		Short term fuel trim
					1,	//0x1C OBD Standards this vehicle conforms to
					1,	//0x1D Oxygen sensors present
					1,	//0x1E Auxiliary input status
					2,	//0x1F Run time since engine start
					4,	//0x20 PIDs supported[21-40]
					2,	//0x21 Distance traveled with malfunction
						//			indicator lamp (MIL) on
					2,	//0x22 Fuel Rail Pressure (relative to manifold vacuum)
					2,	//0x23 Fuel Rail Pressure (diesel, or gasonline direct inject)
					4,	//0x24 O2S1_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x25 O2S2_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x26 O2S3_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x27 O2S4_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x28 O2S5_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x29 O2S6_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x2A O2S7_WR_lambda(1): Equivalence Ratio Voltage
					4,	//0x2B O2S8_WR_lambda(1): Equivalence Ratio Voltage
					1,	//0x2C Commanded EGR
					1,	//0x2D EGR Error
					1,	//0x2E Commanded evaporative purge
					1,	//0x2F Fuel Level Input
					1,	//0x30 # of warm-ups since codes cleared
					2,	//0x31 Distance traveled since codes cleared
					2,	//0x32 Evap. System Vapor Pressure
					1,	//0x33 Barometric pressure
					4,	//0x34 O2S1_WR_lambda(1): Equivalence Ratio Current
					4,	//0x35 O2S2_WR_lambda(1): Equivalence Ratio Current
					4,	//0x36 O2S3_WR_lambda(1): Equivalence Ratio Current
					4,	//0x37 O2S4_WR_lambda(1): Equivalence Ratio Current
					4,	//0x38 O2S5_WR_lambda(1): Equivalence Ratio Current
					4,	//0x39 O2S6_WR_lambda(1): Equivalence Ratio Current
					4,	//0x3A O2S7_WR_lambda(1): Equivalence Ratio Current
					4,	//0x3B O2S8_WR_lambda(1): Equivalence Ratio Current
					2,	//0x3C Catalyst Temperature Bank 1, Sensor 1
					2,	//0x3D Catalyst Temperature Bank 2, Sensor 1
					2,	//0x3E Catalyst Temperature Bank 1, Sensor 2
					2,	//0x3F Catalyst Temperature Bank 2, Sensor 2
					4,	//0x40 PIDs supported [41-60]
					4,	//0x41 Monitor status this drive cycle
					2,	//0x42 Control module voltage
					2,	//0x43 Absolute load value
					2,	//0x44 Command equivalence ratio
					1,	//0x45 Relative throttle position
					1,	//0x46 Ambient air temperature
					1,	//0x47 Absolute throttle position B
					1,	//0x48 Absolute throttle position C
					1,	//0x49 Absolute throttle position D
					1,	//0x4A Absolute throttle position E
					1,	//0x4B Absolute throttle position F
					1,	//0x4C Command throttle actuator
					2,	//0x4D Time run with MIL on
					2,	//0x4E Time since trouble codes cleared
					4,	//0x4F Maximum value for equivalence ratio, Oxygen Sensor
						//		Voltage, oxygen sensor current, and intake manifold
						//		absolute pressure
					4,	//0x50 Maximum value for air flow rate from mass air
						//		flow sensor
					1,	//0x51 Fuel Type
					1,	//0x52 Ethanol fuel %
					2,	//0x53 Absolute Evap system Vapor Pressure
					2,	//0x54 Evap system vapor pressure
					2,	//0x55 Short term secondary oxygen sensor trim bank 1 and bank 3
					2,	//0x56 Long term secondary oxygen sensor trim bank 1 and bank 3
					2,	//0x57 Short term secondary oxygen sensor trim bank 2 and bank 4
					2,	//0x58 Long term secondary oxygen sensor trim bank 2 and bank 4
					2,	//0x59 Fuel rail pressure (absolute)
					1,	//0x5A Relative accelerator peda1 position
					1,	//0x5B Hybrid battery pack remaining life
					1,	//0x5C Engine oil temperature
					2,	//0x5D Fuel injection timing
					2,	//0x5E Engine fuel rate
					1,	//0x5F Emission requirements to which vehicle is designed
						
	};

	
const int map_para1[][6] = {4,8,7,8,2,7}; 
const int map_para2[6] = {1,10,100,1000,10000,100000};
//const int maf_para = 108336;
const int maf_para = 923;

unsigned char temp[512];

int ecu_num = 0;

int processSTNreturn(char * result, int count, int index, int val)
{
	unsigned char content[512];
	memset(content, 0, 512);
	memcpy(content, result, count);
	switch(index){
		case TRY_AUTO_PROTOCOL:
			printf("[Try_auto_protocol]>[%d]: %s\n", count, content);
			break;
		case GET_PROTOCOL:
			printf("[Get_protocol]>[%d]: %s\n", count, content);
			break;		
		case TRY_PROTOCOL:
			printf("[Try_protocol-%x]>[%d]: %s\n", val, count, content);
			break;
		case SET_PROTOCOL:
			printf("[Set_protocol-%x]>[%d]: %s\n", val, count, content);
			break;	
		case GET_PROTOCOLNUMBER:
			printf("[Get_protocolnumber]>[%d]: %s\n", count, content);
			break;
		case GET_VOLTAGE:
			printf("Battery Voltage: %s\n", content);
			break;
		case SET_BRD_Terminal:
			printf("[Change Baudrate]>[%d]: %s\n", count, content);
			break;
		case SET_BRTONVM:
			printf("[Set BR to NVM]>[%d]: %s\n", count, content);
			break;
		case AT_RESET:
			printf("STN1110 Reset>[%d]: %s\n", count, content);
			break; 
		case SET_HEADER:
			printf("[Set Header %d]>[%d]: %s\n", val, count, content);
			break;
		default:
			break;
	}
}

int ReadFromOBD(int fd,char *rxbuf, unsigned long timeout){

    char *bufptr;
    int nread=0;
    int rxnum=0;

    bufptr = rxbuf;

    while(timeout--){

        nread = read(fd,bufptr,rxbuf_size+rxbuf-bufptr-1);

        if(nread > 0)
            bufptr+=nread;

        if(bufptr[-1]==0x3e)
            break;
    }

    if(timeout<=0){

        printf("Read from OBD Timeout!!!\n");
        return -1;
    }
    else{

        rxnum = bufptr - rxbuf;
    }
	tcflush(fd, TCIOFLUSH);
	return rxnum;
}




int STNProcessMessage(int spnum, char *msg, int len){

    int i,j=0,index;
    char *pos1;
    char *pos2;
    int msgnum=0;
    int total = 0;

	if(spnum == 0){
            pos1 = strchr(msg,0x0A);

            msgnum= pos1 -msg;
				for(i=0; i<msgnum; i++){
					msg[i] = pos1[i];
				}
            msg[msgnum]=0x00;
	}
	else{
		pos1 = strchr(msg,0x0A);
			for(i = 0; i < spnum - 1; i++){
				pos1 = strchr(pos1+1, 0x0A); 				
			}
		pos2 = strchr(pos1+1,0x0A);

		msgnum= pos2 -pos1-1;

		for(i=0;i<msgnum;i++){

			msg[i]=pos1[i+1];
			}
		msg[msgnum]=0x00;
    }
    	
    return msgnum;
}

int SerialCommand(int fd, char *txbuf, int count, char *rxbuf)
{
    int nwrite=0;
    int rxnum=0;
    int msgnum=0;

	tcflush(fd, TCIOFLUSH);
    //Write Command
    nwrite = write(fd,txbuf,count);

    if(nwrite < count){

        printf("Send AT Command Failed,Sent %d byte(s)!\n",nwrite);
        return -1;
    }

   //Read from OBD
	memset(rxbuf, 0, rxbuf_size);
	if(strncmp(txbuf,"01 00", 5) == 0) 
		rxnum = ReadFromOBD(fd,rxbuf, 200000);
	else if(strncmp(txbuf, "09 0", 4) == 0)
		rxnum = ReadFromOBD(fd,rxbuf, 50000);
	else
		rxnum = ReadFromOBD(fd,rxbuf, 20000);
	//Process Data
	//msgnum = ProcessMessage(1,rxbuf,rxnum);

    return rxnum;
}

void Print_Datas(unsigned char *data, int len, unsigned char *message)
{
	int index;
	printf("%s", message);
	for(index = 0; index < len; index++)
	{
		if( (index % 16) == 0 )	printf("\n");
		printf("%02X ", data[index]);
	}
	printf("\n");
}

void Cal_Vehicle_CID(unsigned char *data)
{
	unsigned char *pVIN, *pSN, *pCALID, *pCID;
	int index, result;
	
	pVIN = data + CONFIG_VIN_ADDR + VIN_LEN - CAL_CID_LEN;
	pSN	  = data + CONFIG_SN_ADDR;
	pCALID = data + CONFIG_CALID_ADDR;
	pCID = data + CONFIG_CID_ADDR;

	for(index = 0; index < CAL_CID_LEN; index++){
		result = (pVIN[index] + pCALID[CID_LEN-index-1]) * pSN[SN_LEN-index-1];
		pCID[index*2]   = (unsigned char)((result & 0xFF00) >> 8);
		pCID[index*2+1] = (unsigned char)(result & 0x00FF);
	}
	
}

void Add_Config_CheckSum(unsigned char *data)
{
	int index;
	unsigned char checksum = 0;
	for(index = 0; index < CONFIG_LENGTH - 1; index++){
		checksum ^= data[index];
	}
	data[CONFIG_CHECKSUM_ADDR] = checksum;
}

int STN_Check_Connect(int fd, unsigned char *rxbuf, unsigned char alpha)
{
	/* This Function is used before Message Header Turn On */
	int ret = 0;

	ret = SerialCommand(fd, "01 00\r", 6, rxbuf);
	
	/*接收超时，返回失败*/
	if(ret <= 0) 
	{
		if(alpha) printf("Receive Failed!\n");
		return 0;
	}
	else
		if(alpha) printf("%s\n",rxbuf);

	/*结果有可能有2种情况，所以此处要使用一个临时缓存*/
	memset(temp, 0, 512);
	memcpy(temp, rxbuf, ret);

	/* 返回第一行数据即为OBD数据，表示连接成功*/
	STNProcessMessage(1, temp, ret);
	if( strncmp(temp, "41 00", 5) == 0 ) 
	{
		if(alpha) printf("OBD>%s\n", temp);
		return 1;
	}

	/*第一行数据有可能返回 search...,所以要继续判断第二行数据*/
	STNProcessMessage(2, rxbuf, ret);
	if( strncmp(rxbuf, "41 00", 5) == 0 ) 
	{
		if(alpha) printf("OBD>%s\n", ret);
		return 1;
	}

	if(alpha) printf("Failed!\n");
	return 0;
}

/*
	Use CAN_Check_Connect to Detect OBD Connection
		if return message length is over CAN_0100_H0N_LEN, return true
*/
int CAN_Check_Connect(int fd, unsigned char *rxbuf, unsigned char alpha)
{
	/* This Function is used after Message Header Turn On */
	int ret = 0;
	unsigned char *ptr;

	memset(temp, 0, 512);
	ret = SerialCommand(fd, "01 00\r", 6, rxbuf);
	if(alpha) printf("OBD>%s\n", temp);
	if( ret >= CAN_0100_HON_LEN ) return 1;
	return 0;
}

int KWP_Check_Connect(int fd, unsigned char *rxbuf, unsigned char alpha)
{
	int ret = 0;
	unsigned char *ptr;

	memset(temp, 0, 512);
	ret = SerialCommand(fd, "01 00\r", 6, rxbuf);
	if(alpha) printf("OBD>%s\n", temp);
	if( ret >= KWP_MSG_LEN ) return 1;
	return 0;
}

unsigned char STN_Check_Protocol(int fd, unsigned char *rxbuf, unsigned char alpha, \
 											unsigned char *data)
{
	int ret = 0;
	unsigned char protocolnumber = 0;
	char * ptr = data;
	ret = AT_Get_ProtocolNumber(fd, rxbuf);
	if( ret == 0) return -1;
	if( alpha )
	if( ret > 0) printf("protocol number: [%02s]\n", rxbuf);
	{
		data[0] = HexToChar(rxbuf[0]) * 16 + HexToChar(rxbuf[1]);
		protocolnumber = data[0] & 0x0F;
	}
	ptr++;
	memcpy(ptr, PROTOCOLNAME[protocolnumber], 15);
	return protocolnumber;
}

/* Using CAN Protocol, 
		We should confirm how many ECU are Vehicle have, when turn header on
*/
int CAN_Judge_ECUS(int fd, unsigned char *rxbuf, unsigned char alpha)
{
	int ret = 0;
	int total = 0;
	unsigned char Fst_ECU_Addr[4];
	unsigned char Sec_ECU_Addr[4];	
	unsigned char *ptr;
	unsigned char reg;
	unsigned char engine_ecu = 0;
	unsigned char mask_cmd[20];
	
	memset(Fst_ECU_Addr, 0, 4);
	memset(Sec_ECU_Addr, 0, 4);
	memset(mask_cmd, 0, 20);
	ecu_num = 0;
	/* Copy ECU Return to Temp buffer */
	memset(temp, 0, 512);
	total = SerialCommand(fd, "01 00\r", 6, rxbuf);
	memcpy(temp, rxbuf, total);
	
	if(alpha) printf ("ECU JUDGE OBD[%d]> %s\n", total, rxbuf);
	
	if( total >= CAN_0100_HON_LEN && total < 2 * CAN_0100_HON_LEN ){
		/* Only One ECU return */
		STNProcessMessage(1, temp, total);
		if(alpha) printf("1 ECU OBD[%d]>%s\n", total, temp);
		if( strncmp(temp, "7E8", 3) == 0 ) {
			memcpy(Fst_ECU_Addr, temp, 3);
			ecu_num = 1;
			printf("Tested only one ECU on this Vechile!\n");
			sprintf(Fst_ECU_Addr, "%s", "7E8");
			sprintf(mask_cmd, "AT CRA %s\r", Fst_ECU_Addr);
			if(alpha) printf("Engine ECU Address: %s \n", Fst_ECU_Addr);
			SerialCommand(fd, mask_cmd, 11, rxbuf);
			return 1;	
		}
		else{
			/*
			STNProcessMessage(2, rxbuf, total);
			if( strncmp(rxbuf, "7E8", 2) == 0) {
				memcpy(Fst_ECU_Addr, temp, 3);
				ecu_num = 1;
				return 1;
			}
			else{
				return 0;
			}	*/
			return 0;
		}
	}
	else if( total >= 2 * CAN_0100_HON_LEN ) {
		/* Vehicle With 2 ECUS*/
		STNProcessMessage(1, temp, total);
		if(alpha) printf("2 ECU OBD[%d]>%s\n", total, temp);

		/* First Return Message is ECU retrun */
		if( strncmp(temp, "7E", 2) == 0 ) {
			if(strncmp(temp, "7E8", 3) == 0)
			{
				/* Get First ECU Addr */
				memcpy(Fst_ECU_Addr, temp, 3);
				//ptr = temp + CAN_0100_RES_OFFSET;
				//reg = HexToChar(ptr[0]) * 16 + HexToChar(ptr[1]);
				/* First ECU is Engine ECU*/
				//if(reg & 0x10 == 0x10) engine_ecu = 1;
				engine_ecu  = 1;
				ecu_num = 1;
			}
			else
			{
				/* Get First ECU Addr */
				memcpy(Fst_ECU_Addr, temp, 3);
				//ptr = temp + CAN_0100_RES_OFFSET;
				//reg = HexToChar(ptr[0]) * 16 + HexToChar(ptr[1]);
				/* First ECU is Engine ECU*/
				//if(reg & 0x10 == 0x10) engine_ecu = 1;
				//engine_ecu  = 1;
				ecu_num = 1;
			}
			STNProcessMessage(2, rxbuf, total);
			if( strncmp(rxbuf, "7E8", 3) == 0 ) {
				/* Get Second ECU Addr */
				ecu_num = 2;
				memcpy(Sec_ECU_Addr, rxbuf, 3);
				//ptr = rxbuf + CAN_0100_RES_OFFSET;
				//reg = HexToChar(ptr[0]) * 16 + HexToChar(ptr[1]);
				/* Second ECU is Engine ECU*/
				//if(reg & 0x10 == 0x10) engine_ecu = 2;
				engine_ecu  = 2;
				ecu_num = 2;	
			}
			else
			{
				memcpy(Sec_ECU_Addr, rxbuf, 3);
				ecu_num = 2;
				}
			/* Second Return Message is Error Message */
			/* If There are more than 2 ECU We should add more Code here to handle */			
		}
		
	}
	else
		return 0;

	/* If there are 2 ECU, we should mask CAN addr to show the Engine ECU message Only */	
	if( ecu_num == 2) {
		/*
		if(engine_ecu == 1) {
			sprintf(mask_cmd, "AT CRA %s\r", Fst_ECU_Addr);
			if(alpha) printf("Engine ECU Address: %s \n", Fst_ECU_Addr);
			SerialCommand(fd, mask_cmd, 11, rxbuf);
		}
		else if(engine_ecu == 2) {
			sprintf(mask_cmd, "AT CRA %s\r", Sec_ECU_Addr);
			if(alpha) printf("Engine ECU Address: %s \n", Sec_ECU_Addr);
			SerialCommand(fd, mask_cmd, 11, rxbuf);
		}*/
		if(engine_ecu != 0)
		{
			sprintf(Fst_ECU_Addr, "%s", "7E8");
			sprintf(mask_cmd, "AT CRA %s\r", "7E8");
			//if(alpha) printf("Engine ECU Address: %s \n", Fst_ECU_Addr);
			if(alpha) printf("Engine ECU Addr Masked!\n");
			SerialCommand(fd, mask_cmd, 11, rxbuf);
		}
		else{
			if(alpha) printf("Engine ECU Address: Not Found\n", Sec_ECU_Addr);
			return 0;
		}
	}
	else
	{
		if(alpha) printf("Only One ECU\n");
			sprintf(Fst_ECU_Addr, "%s", "7E8");
			sprintf(mask_cmd, "AT CRA %s\r", "7E8");
			//if(alpha) printf("Engine ECU Address: %s \n", Fst_ECU_Addr);
			if(alpha) printf("Engine ECU Addr Masked!\n");
			SerialCommand(fd, mask_cmd, 11, rxbuf);
		}		
	return ecu_num;	
}

/* Link CAN Mulit Line message to One 
	return: valid mesage length
*/ 
int Org_CAN_Message(unsigned char *data, int data_length, unsigned char * newdata, int lines)
{
	char *ptr = data;
	int index = 0;
	int count_line = 0;
	int total_length = 0;
	int count_data = 0;
	/*Calculate lines of message*/
	for(count_line = 1; count_line <=lines; count_line++){
		memset(temp, 0, 512);
		memcpy(temp, data, data_length);
		/* Process each line in Return Message */	
		STNProcessMessage(count_line, temp, data_length);
		/* Initial Pointer */
		ptr = temp;
		/* Process First need to Get Total Length */
		if(count_line == 1){
			ptr += CAN_HEAD_LEN;	// 7E8 10 NN CC CC DD DD DD DD 
										// * NN is number of Message
										// * CC is Command Return
										//	* DD is Data
			if(lines == 1) ptr-= 3; // if only one can message return
													   // 7E8 NN CC CC DD ...
													   // ptr should move back a byte with
													   // a blank mark
			
			total_length = HexToChar(ptr[0]) * 16 + HexToChar(ptr[1]);	// Get NN
    		ptr += BYTE_LEN + 1;	// Move pointer to CC 
			
			/* Get CC & DD from message, 10 Message Content Length is 18 */
			if(lines == 1){
				for(index = 0; index < CAN_ONE_MSG_LEN; index++){
					newdata[index] = ptr[index];
					count_data++;
				}
			}
			else
			{

				for(index = 0; index < CAN_10_MSG_LEN; index++){
					newdata[index] = ptr[index];
					count_data++;
				}
			}
		}
		else{
			/* Move pointer jump to first DD */
			ptr += CAN_HEAD_LEN;	// 7E8 2X DD DD DD DD DD DD DD	
										//	* DD is data
			/* Get CC & DD from message, 2X Message Content Length is 21 */							
			for(index = 0; index < CAN_2X_MSG_LEN; index++){
				newdata[count_data++] = ptr[index];
			}
		}	
	}
	return total_length;	// Return Actual Length of Content With Command Return message
}


/*
	Get PID List from Vehicle
*/
int CAN_Get_PID_Support(int fd, unsigned char *rxbuf, unsigned char alpha, unsigned char * data)
{
	int ret = 0;
	int index = 0;
	int count_PID = 0;
	int message_length = 0; // return content length
	int message_count = 0; 	// how many message line of in return message
	char can_data[512];
	unsigned char *ptr;
	int flag_pid00 = 0;
	memset(can_data, 0, 512);

	/* Clear Buffer on PID Position */
	for(index = 0; index < 16; index++) data[index+0x40] = 0;

	/* Get PID Support 01 01 - 01 5F (hex) */
	ret = SerialCommand(fd, "01 00 20 40\r", 12, rxbuf);

	/* return message count mod 28, if not equal 0, return false */
	if(alpha) printf("[OBD]%s\n", rxbuf);
	if( strncmp(rxbuf, "01 00 20 40", 11) != 0)
	{
		if(alpha) printf("CAN_Get_PID_Support Error[ret:%d]:\n", ret);
		return -1; //13 means ">01 00 20 40\r"
	}
	/* Get how many lines in return message */
	message_count = (ret - 12) / CAN_MSG_LEN;

	if( message_count <= 0 ) return -1;

	/* Process Return Message */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, message_count);

	if( alpha ) printf("OBD>%s\n", can_data);
	
	/* Initial pointer */	
	ptr = can_data;
	
	if( strncmp(ptr, "41 ", 3) == 0) {
		/* Cal How many PID responsed from ECU */
		//count_PID = (message_length - BYTE_LEN - 1) / CAN_PID_MSG_LEN; // CAN_PID_MSG_LEN is 15
		count_PID = 3;
		/* Move point to the first PID Message */
		ptr+= BYTE_LEN + 1;		// plus 1 to jump a space
		
		for(index = 0; index < count_PID; index++){
			/*	requested PID 00 */
			if( strncmp(ptr, "00 ", 3) == 0 && flag_pid00 == 0){
				data[0x40] = HexToChar(ptr[3]) * 16 + HexToChar(ptr[4]);
				data[0x41] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
				data[0x42] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
				data[0x43] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
				
				if(data[0x41] & 0x20) data[0x51] = 0x0B;	// 01 0B is Existed,	MAP
				if(data[0x41] & 0x01) data[0x51] = 0x10;	// 01 10 is Existed, 	MAF
				printf("PID00: %02X, %02X, %02X, %02X\n",data[0x40],data[0x41],data[0x42], data[0x43]);
				flag_pid00 = 1;
			}
			/* requested PID 20 */
			if( strncmp(ptr, "20 ", 3) == 0) {
				data[0x44] = HexToChar(ptr[3]) * 16 + HexToChar(ptr[4]);
				data[0x45] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
				data[0x46] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
				data[0x47] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);

				if( data[0x44] & 0x80 )	data[0x54] = 0x21;		// 01 21 is Existed, 
				if(	 data[0x45] & 0x02 )	data[0x53] = 0x2F;		// 01 2F is Existed,
				if( data[0x46] & 0x80 )	data[0x55] = 0x31;		// 01 31 is Existed,
				printf("PID20: %02X, %02X, %02X, %02X\n",data[0x44],data[0x45],data[0x46], data[0x47]);
			}
			/* requested PID 40 */
			if( strncmp(ptr, "40 ", 3) == 0) {
				data[0x48] = HexToChar(ptr[3]) * 16 + HexToChar(ptr[4]);
				data[0x49] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
				data[0x4A] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
				data[0x4B] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
				if( data[0x51] == 0x0B && ((data[0x49] & 0x02) > 0) )
					data[0x52] = 0x4F;									// 01 4F is Existed,

				if( data[0x51] == 0x10 && ((data[0x49] & 0x01) > 0) )
					data[0x52] = 0x50; 								// 01 50 is Existed,
			
				if( data[0x4B] & 0x04	)	data[0x50] = 0x5E;		// 01 5E is Existed,
				printf("PID40: %02X, %02X, %02X, %02X\n",data[0x48],data[0x49],data[0x4A], data[0x4B]);	
			}

			ptr += CAN_PID_MSG_LEN;	
		}// for
	}//if
	else return -1;
	
	/* Get PID Support 09 01 - 09 20 (hex) */
	memset(rxbuf, 0, rxbuf_size);
	ret = SerialCommand(fd, "09 00\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	/* Point in CAN Protocol should be repoint */
	ptr = rxbuf + CAN_HEAD_LEN; 

	if( alpha ) printf("OBD>%s\n", rxbuf);
	if( strncmp(ptr, "49 00", 5) == 0 && ret >= CAN_RES_MSG_LEN + CAN_PID_MSG_LEN){
		/* Response from ECU 49 00 MsgCount XX */
		data[0x4C] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		//data[0x4D] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
		//data[0x4E] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
		//data[0x4F] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
	}
	else return -4;
	
	return 0;
}

int KWP_Get_PID_Support(int fd, unsigned char *rxbuf, unsigned char alpha, unsigned char * data)
{
	int ret = 0;
	int index = 0;
	char *ptr = rxbuf;

	// Get PID Support 01 01 - 01 20 (hex)
	ret = SerialCommand(fd, "01 00\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if( alpha ) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 00", 5) == 0 && ret >= KWP_MSG_LEN){
		data[0x40] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		data[0x41] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
		data[0x42] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
		data[0x43] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
	}
	else return -1;
	
	if(data[0x41] & 0x20) data[0x51] = 0x0B;	// 01 0B is Existed,	MAP
	if(data[0x41] & 0x01) data[0x51] = 0x10;	// 01 10 is Existed, 	MAF

	if(data[0x43] & 0x01){
		// Get PID Support 01 21 - 01 40 (hex)
		memset(rxbuf, 0, rxbuf_size);
		ret = SerialCommand(fd, "01 20\r", 6, rxbuf);
		STNProcessMessage(1, rxbuf, ret);
		if( alpha ) printf("OBD>%s\n", rxbuf);
		if( strncmp(ptr, "41 20", 5) == 0 && ret >= KWP_MSG_LEN){
			data[0x44] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
			data[0x45] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
			data[0x46] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
			data[0x47] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
		}
		else return -2;
		
		if( data[0x44] & 0x80 )	data[0x54] = 0x21;		// 01 21 is Existed, 
		if(	 data[0x45] & 0x02 )	data[0x53] = 0x2F;		// 01 2F is Existed,
		if( data[0x46] & 0x80 )	data[0x55] = 0x31;		// 01 31 is Existed,
	
		if( data[0x47] & 0x01 ){
			// Get PID Support 01 41 - 01 60 (hex)
			memset(rxbuf, 0, rxbuf_size);
			ret = SerialCommand(fd, "01 40\r", 6, rxbuf);
			STNProcessMessage(1, rxbuf, ret);
			if( alpha ) printf("OBD>%s\n", rxbuf);
			if( strncmp(ptr, "41 40", 5) == 0 && ret >= KWP_MSG_LEN){
				data[0x48] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
				data[0x49] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
				data[0x4A] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
				data[0x4B] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
			}
			else return -3;		
			
			if( data[0x51] == 0x0B && ((data[0x49] & 0x02) > 0) )
				data[0x52] = 0x4F;	// 01 4F is Existed,

			if( data[0x51] == 0x10 && ((data[0x49] & 0x01) > 0) )
				data[0x52] = 0x50; 	// 01 50 is Existed,
			
			if( data[0x4B] & 0x04	)	data[0x50] = 0x5E;		// 01 5E is Existed,
		}
		else{
			for(index = 0; index < 4; index ++) {
				data[0x48+index] = 0;
			}
		}
	}
	else{
		for(index = 0; index < 8; index++){
			data[0x44+index] = 0;
		}
	}

	/* Get PID Support 09 01 - 01 20 (hex) */
	memset(rxbuf, 0, rxbuf_size);
	ret = SerialCommand(fd, "09 00\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if( alpha ) printf("OBD>%s\n", rxbuf);
	if( strncmp(ptr, "49 00", 5) == 0 && ret >= KWP_MSG_LEN){
		/* 09 00  Reponse message 49 00 MsgCount XX XX XX XX */
		data[0x4C] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
		//data[0x4D] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
		//data[0x4E] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
		//data[0x4F] = HexToChar(ptr[18]) * 16 + HexToChar(ptr[19]);
	}
	else return -4;
	
	return 0;
}

/* Use CAN Potocol to Get VIN of Vehicle 
	return: VIN length of "49 02 DD DD DD ...."
*/
int CAN_Get_VIN(int fd, unsigned char *rxbuf, unsigned char alpha, unsigned char * data)
{
	int ret = 0;
	int index = 0;
	char tmpVIN[VIN_LEN+1];
	unsigned char * ptr;
	unsigned char can_data[512];
	int message_length;

	memset(tmpVIN, 0x00, VIN_LEN+1);
	memset(can_data, 0x00, 512);

	/* Get VIN Information */
	if(data[0x4C] & 0x40 == 0) return -1; // Vehicle not support VIN

	ret = SerialCommand(fd, "09 02\r", 6, rxbuf);
	
	/* Process Return Message */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, CAN_VIN_LINES);

	ptr = can_data;

	if( alpha ) printf("OBD>%s\n", can_data);

	if( strncmp(ptr, "49 02", 5) == 0){
		ptr = can_data + 9;	// 6 means "49 02 01 "
		for(index = 0; index < VIN_LEN; index++){
			data[CONFIG_VIN_ADDR + index] = HexToChar(ptr[0]) * 16 + HexToChar(ptr[1]);
			tmpVIN[index] = data[CONFIG_VIN_ADDR + index];
			ptr += BYTE_LEN + 1;
		}
		if( alpha ){
			printf("VIN of Vehicle: %s\n", tmpVIN);
		}
	}else return 0;
	return message_length;	// length of "49 02 DD DD DD ..."
}


/* Use KWP Potocol to Get VIN of Vehicle 
	return: Package of VIN
*/
int KWP_Get_VIN(int fd, unsigned char *rxbuf, unsigned char alpha, unsigned char * data)
{
	int ret = 0;
	int index = 0;
	int package = 0;
	char tempbuf[256];
	char cmdStr[10];
	char tmpVIN[40];
	char * ptr = rxbuf + KWP_HEAD_LEN;
	char * vptr1 = data + CONFIG_VIN_ADDR;
	char * vptr2 = tmpVIN + 3;
 
	memset(tempbuf, 0, 256);
	memset(cmdStr, 0, 10);
	memset(tmpVIN, 0x20, 40);
	//	Get VIN Package Length	
	ret = SerialCommand(fd, "09 01\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if( alpha ) printf("OBD>%s\n", rxbuf);
	if( strncmp(ptr, "49 01", 5) == 0){
		package = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		if( alpha ) printf("VIN package numbers: %d\n", package);
	}else return 0;
	
	if ( package > 0 ){
		memset(rxbuf, 0, rxbuf_size);
		ret = SerialCommand(fd, "09 02\r", 6, rxbuf);
		if( alpha ) printf("OBD>[%d]: %s\n", ret, rxbuf);

		ptr = tempbuf + KWP_HEAD_LEN;
		for(index = 0; index < package; index++){
			memset(tempbuf, 0, 256);
			memcpy(tempbuf, rxbuf, 256);

			STNProcessMessage(index+1, tempbuf, ret);

			sprintf(cmdStr, "49 02 %02x", index+1);

			if( strncmp(ptr, cmdStr, 8) == 0){
				tmpVIN[index*4 + 0] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
				tmpVIN[index*4 + 1] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
				tmpVIN[index*4 + 2] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
				tmpVIN[index*4 + 3] = HexToChar(ptr[18]) * 16 + HexToChar(ptr[19]);
			}else	break;	
		}
		if(index != package) return -1;	
		memcpy(vptr1, vptr2, VIN_LEN);
		return package;
	}
	return 0;
}

/*
	Use CAN Potocol to Get CALID of Vehicle
	return: CALID length of "49 04 DD DD DD ..."
*/
int CAN_Get_CALID(int fd, unsigned char *rxbuf, unsigned char alpha, unsigned char * data)
{

	int ret = 0;
	int index = 0;
	char tmpCALID[CALID_LEN+1];

	unsigned char * ptr;
	unsigned char can_data[512];
	int message_length;

	memset(tmpCALID, 0x00, CALID_LEN+1);
	memset(can_data, 0x00, 512);

	/* Get VIN Information */
	if(data[0x4C] & 0x10 == 0) return -1; // Vehicle not support CALID

	ret = SerialCommand(fd, "09 04\r", 6, rxbuf);

	/* Process Return Message */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, CAN_CALID_LINES);

	if( alpha ) printf("OBD>%s\n", can_data);

	ptr = can_data;

	if( strncmp(ptr, "49 04", 5) == 0){
		ptr = can_data + 9;	// 6 means "49 04 01 "

		for(index = 0; index < CALID_LEN; index++){
			data[CONFIG_CALID_ADDR + index] = HexToChar(ptr[0]) * 16 + HexToChar(ptr[1]);
			tmpCALID[index] = data[CONFIG_CALID_ADDR + index];
			ptr += BYTE_LEN + 1;		
		}
		if( alpha ){
			printf("CALID of Vehicel: %s\n", tmpCALID);
		}
	}else return 0;
	return message_length;	// Length of "49 04 DD DD DD ..."
}

int KWP_Get_CALID(int fd, unsigned char *rxbuf, unsigned char alpha, unsigned char * data)
{
	int ret = 0;
	int index = 0;
	int package = 0;
	char tempbuf[256];
	char cmdStr[10];
	char tmpCALID[40];
	char * ptr = rxbuf + KWP_HEAD_LEN;
	char * vptr1, * vptr2;
 
	memset(tempbuf, 0, 256);
	memset(cmdStr, 0, 10);
	memset(tmpCALID, 0x20, 40);

	//	Get CALID Package Length	
	ret = SerialCommand(fd, "09 03\r", 6, rxbuf);

	STNProcessMessage(1, rxbuf, ret);

	if( alpha ) printf("OBD>%s\n", rxbuf);
	if( strncmp(ptr, "49 03", 5) == 0){
		package = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		if( alpha ) printf("CALID package numbers: %d\n", package);
	}else return 0;
	
	if ( package > 0 ){
		memset(rxbuf, 0, rxbuf_size);
		ret = SerialCommand(fd, "09 04\r", 6, rxbuf);
		if( alpha ) printf("OBD>[%d]: %s\n", ret, rxbuf);

		ptr = tempbuf + KWP_HEAD_LEN;
		for(index = 0; index < package; index++){
			memset(tempbuf, 0, 256);
			memcpy(tempbuf, rxbuf, 256);

			STNProcessMessage(index+1, tempbuf, ret);

			sprintf(cmdStr, "49 04 %02x", index+1);

			if( strncmp(ptr, cmdStr, 8) == 0){
				tmpCALID[index*4 + 0] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
				tmpCALID[index*4 + 1] = HexToChar(ptr[12]) * 16 + HexToChar(ptr[13]);
				tmpCALID[index*4 + 2] = HexToChar(ptr[15]) * 16 + HexToChar(ptr[16]);
				tmpCALID[index*4 + 3] = HexToChar(ptr[18]) * 16 + HexToChar(ptr[19]);
			}else	break;	
		}
		if(index != package) return -1;	
		vptr1 = data + CONFIG_CALID_ADDR;
		vptr2 = tmpCALID;
		memcpy(vptr1, vptr2, CALID_LEN);
		return package;
	}
	return 0;
}

/*
	Use CAN potocol to Get Engine RPM of Vehicle
	return: RPM Value
*/
int CAN_Get_EngineRPM(int fd, unsigned char *rxbuf, unsigned char alpha, \
								struct vehicle_status * vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0C\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 0C", 5) == 0 && ret >= 20){
		vs->VehicleEnginRPM[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->VehicleEnginRPM[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	return 	((vs->VehicleEnginRPM[0]* 256 + vs->VehicleEnginRPM[1]) / 4);
}

/*
	Use KWP potocol to Get Engine RPM of Vehicle
	return: RPM Value
*/
int KWP_Get_EngineRPM(int fd, unsigned char *rxbuf, unsigned char alpha, \
								struct vehicle_status * vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0C\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 0C", 5) == 0 && ret >= 20){
		vs->VehicleEnginRPM[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->VehicleEnginRPM[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	return 	((vs->VehicleEnginRPM[0]* 256 + vs->VehicleEnginRPM[1]) / 4);
}

/*
	Use CAN potocol to Get Engine EFR of Vehicle
	return: EFR Value
*/
int CAN_Get_EngineEFR(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 5E\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 5E", 5) == 0 && ret >= 20){
		vs->EngineFuelRate[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->EngineFuelRate[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* EFR 0 ~ 327575	*/
	return 	(int)((vs->EngineFuelRate[0]* 256 + vs->EngineFuelRate[1]) * 5);
}

/*
	Use KWP potocol to Get Engine EFR of Vehicle
	return: EFR Value
*/
int KWP_Get_EngineEFR(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 5E 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 5E", 5) == 0 && ret >= 20){
		vs->EngineFuelRate[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->EngineFuelRate[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* EFR 0 ~ 327575	*/
	return 	(int)((vs->EngineFuelRate[0]* 256 + vs->EngineFuelRate[1]) * 5);
}

/*
	Use CAN potocol to Get Engine MAP of Vehicle
	return: MAP Value
*/
int CAN_Get_EngineMAP(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0B\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 0B", 5) == 0 && ret >= 17){
		vs->MAP = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;

	/* MAP 0 ~ 255 */
	return 	(int)(vs->MAP);
}

/*
	Use KWP potocol to Get Engine MAP of Vehicle
	return: MAP Value
*/
int KWP_Get_EngineMAP(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0B 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 0B", 5) == 0 && ret >= 17){
		vs->MAP = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;

	/* MAP 0 ~ 255 */
	return 	(int)(vs->MAP);
}

/*
	Use CAN potocol to Get 0x2F Fuel Level Input of Vehicle
	return: Fuel_LV Value
*/
int CAN_Get_Fuel_LV(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 2F\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 2F", 5) == 0 && ret >= 17){
		vs->Fuel_LV = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;

	/* Fuel_LV 0 ~ 255 */
	return 	(int)(vs->Fuel_LV);
}

/*
	Use KWP potocol to Get 0x2F Fuel Level Input of Vehicle
	return: POI Value
*/
int KWP_Get_Fuel_LV(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 2F 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 2F", 5) == 0 && ret >= 17){
		vs->Fuel_LV = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;

	/* Fuel_LV 0 ~ 255 */
	return 	(int)(vs->Fuel_LV);
}


/*
	Use CAN potocol to Get Engine IAT of Vehicle
	return: IAT Value
*/
int CAN_Get_EngineIAT(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status * vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0F\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 0F", 5) == 0 && ret >= 17){
		vs->IntakeAirTemp = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* IAT -40 ~ 215 */
	return 	(int)(vs->IntakeAirTemp - 40);
}

/*
	Use KWP potocol to Get Engine IAT of Vehicle
	return: IAT Value
*/
int KWP_Get_EngineIAT(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status * vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0F 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 0F", 5) == 0 && ret >= 17){
		vs->IntakeAirTemp = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* IAT -40 ~ 215 */
	return 	(int)(vs->IntakeAirTemp - 40);
}

/*
	Use CAN potocol to Get Engine STFT of Vehicle
	return: STFT Value
*/
int CAN_Get_EngineSTFT(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 06\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 06", 5) == 0 && ret >= 17){
		vs->ECU_STFuelTrim = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;

	/* STFT -10000 ~ 9921 */
	return 	(int)((vs->ECU_STFuelTrim - 128) * 100 * 100 / 128);
}

/*
	Use KWP potocol to Get Engine STFT of Vehicle
	return: STFT Value
*/
int KWP_Get_EngineSTFT(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 06 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 06", 5) == 0 && ret >= 17){
		vs->ECU_STFuelTrim = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;

	/* STFT -10000 ~ 9921 */
	return 	(int)((vs->ECU_STFuelTrim - 128) * 100 * 100 / 128);
}

/*
	Use CAN potocol to Get Engine LTFT of Vehicle
	return: LTFT Value
*/
int CAN_Get_EngineLTFT(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 07\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 07", 5) == 0 && ret >= 17){
		vs->ECU_LTFuelTrim = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	/* LTFT -10000 ~ 9921 */
	return 	(int)((vs->ECU_LTFuelTrim - 128) * 100 * 100/ 128);
}

/*
	Use KWP potocol to Get Engine LTFT of Vehicle
	return: LTFT Value
*/
int KWP_Get_EngineLTFT(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 07 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 07", 5) == 0 && ret >= 17){
		vs->ECU_LTFuelTrim = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	/* LTFT -10000 ~ 9921 */
	return 	(int)((vs->ECU_LTFuelTrim - 128) * 100 * 100/ 128);
}

/*
	Use CAN potocol to Get Engine MAF of Vehicle
	return: MAF Value
*/
int CAN_Get_EngineMAF(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 10\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 10", 5) == 0 && ret >= 17){
		vs->AirFlowRate[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->AirFlowRate[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* maf 0-65535 */
	return 	(int)((vs->AirFlowRate[0] * 256 + vs->AirFlowRate[1]));
}

/*
	Use KWP potocol to Get Engine MAF of Vehicle
	return: MAF Value
*/
int KWP_Get_EngineMAF(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 10 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 10", 5) == 0 && ret >= 17){
		vs->AirFlowRate[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->AirFlowRate[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* maf 0-65535 */
	return 	(int)((vs->AirFlowRate[0] * 256 + vs->AirFlowRate[1]));
}

/*
	Use CAN potocol to Get SPEED of Vehicle
	return: SPEED Value
*/
int CAN_Get_SPEED(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0D\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 0D", 5) == 0 && ret >= 17){
		vs->VehicleSpeed = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* speed 0-255 */
	return 	(int)(vs->VehicleSpeed);
}

/*
	Use KWP potocol to Get SPEED of Vehicle
	return: SPEED Value
*/
int KWP_Get_SPEED(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 0D 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 0D", 5) == 0 && ret >= 17){
		vs->VehicleSpeed = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* speed 0-255 */
	return 	(int)(vs->VehicleSpeed);
}

/*
	Use CAN potocol to Get Engine Load of Vehicle
	return: Engine Load Value
*/
int CAN_Get_ENGINE_LOAD(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 04\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 04", 5) == 0 && ret >= 17){
		vs->Engine_Load = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* Engine_Load % */
	return 	(int)(vs->Engine_Load * 100 / 255);
}

/*
	Use KWP potocol to Get Engine Load of Vehicle
	return: Engine Load Value
*/
int KWP_Get_ENGINE_LOAD(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 04 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 04", 5) == 0 && ret >= 17){
		vs->Engine_Load = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* Engine_Load % */
	return 	(int)(vs->Engine_Load * 100 / 255);
}

/*
	Use CAN potocol to Get Coolant temperature of Vehicle
	return: temperature Value
*/
int CAN_Get_COOL_TEMP(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 05\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 05", 5) == 0 && ret >= 17){
		vs->Cool_Temp = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* COOL_TEMP A-40 @here not do sub 40, because this parameter don't join complex calculation */
	return 	(int)(vs->Cool_Temp);
}


/*
	Use KWP potocol to Get Coolant temperature of Vehicle
	return: temperature Value
*/
int KWP_Get_COOL_TEMP(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 05 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 05", 5) == 0 && ret >= 17){
		vs->Cool_Temp = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* COOL_TEMP A-40 @here not do sub 40, because this parameter don't join complex calculation */
	return 	(int)(vs->Cool_Temp);
}

/*
	Use CAN potocol to Get Pad Position of Vehicle
	return: Pad Position Value
*/
int CAN_Get_PAD_POS(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 11\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 11", 5) == 0 && ret >= 17){
		vs->Pad_Position = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* Pad_Position % */
	return 	(int)(vs->Pad_Position * 100 / 255);
}

/*
	Use CAN potocol to Get Pad Position of Vehicle
	return: Pad Position Value
*/
int KWP_Get_PAD_POS(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 11 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 11", 5) == 0 && ret >= 17){
		vs->Pad_Position = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
	}
	else return STN_NONE_RETURN;
	
	/* Pad_Position % */
	return 	(int)(vs->Pad_Position * 100 / 255);
}

/*
	Use CAN potocol to Get Distance of register 01 21 of Vehicle
	return: Distance Value
*/
int CAN_Get_DIST21(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 21\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 21", 5) == 0 && ret >= 17){
		vs->Dist_Cal21[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->Dist_Cal21[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* maf 0-65535 */
	return 	(int)((vs->Dist_Cal21[0] * 256 + vs->Dist_Cal21[1]));
}

/*
	Use KWP potocol to Get Distance of register 01 21 of Vehicle
	return: Distance Value
*/
int KWP_Get_DIST21(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 21 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 21", 5) == 0 && ret >= 17){
		vs->Dist_Cal21[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->Dist_Cal21[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* maf 0-65535 */
	return 	(int)((vs->Dist_Cal21[0] * 256 + vs->Dist_Cal21[1]));
}

/*
	Use CAN potocol to Get Distance of register 01 31 of Vehicle
	return: Distance Value
*/
int CAN_Get_DIST31(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 31\r", 6, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += CAN_HEAD_LEN; 
	if( strncmp(ptr, "41 31", 5) == 0 && ret >= 17){
		vs->Dist_Cal31[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->Dist_Cal31[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* maf 0-65535 */
	return 	(int)((vs->Dist_Cal31[0] * 256 + vs->Dist_Cal31[1]));
}

/*
	Use KWP potocol to Get Distance of register 01 31 of Vehicle
	return: Distance Value
*/
int KWP_Get_DIST31(int fd, unsigned char *rxbuf, unsigned char alpha, \
											struct vehicle_status *vs)
{
	int ret = 0;
	char * ptr = rxbuf;

	ret = SerialCommand(fd, "01 31 1\r", 8, rxbuf);
	STNProcessMessage(1, rxbuf, ret);
	if(alpha) printf("OBD>%s\n", rxbuf);
	ptr += KWP_HEAD_LEN; 
	if( strncmp(ptr, "41 31", 5) == 0 && ret >= 17){
		vs->Dist_Cal31[0] = HexToChar(ptr[6]) * 16 + HexToChar(ptr[7]);
		vs->Dist_Cal31[1] = HexToChar(ptr[9]) * 16 + HexToChar(ptr[10]);
	}
	else return STN_NONE_RETURN;
	
	/* maf 0-65535 */
	return 	(int)((vs->Dist_Cal31[0] * 256 + vs->Dist_Cal31[1]));
}

/* 通过单条CAN指令读取多个OBD寄存器获得驾驶行为的相关参数
 * 读取的寄存器包括04 05 11三个寄存器
 * 2014/01/26已经在2013款FOCUS车上验证通过
 */ 
int CAN_GetMultiDriveBehavior(int fd, unsigned char *rxbuf, unsigned char alpha, \
						struct vehicle_status * vs)
{
	int ret = 0;
	unsigned char *ptr;
	unsigned char can_data[512];
	int message_length;
	int index;
	int pid_index;
	int pid_cycle;
	unsigned char pid_data[4];
	unsigned char *ptr_data;
	
	memset(can_data, 0x00, 512);
	ret = SerialCommand(fd, "01 04 05 11\r", 12, rxbuf);
	printf("Driver Behavior Length: %d/[%d]\n", ret,CAN_MULTI_DBH_TOTAL);
	printf("[Driver Behavior]OBD>%s\n", rxbuf);
	if(ret != CAN_MULTI_DBH_TOTAL) return -1;
	/* Process Return Messagsr */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, CAN_MULTI_DBH_LINES);	
	printf("CAN Data[%d/%d]> %s\n", message_length, CAN_MULTI_DBH_RETURN_DATA,can_data);
	if(message_length != CAN_MULTI_DBH_RETURN_DATA) return -1;
	/* Expect Data format:
	*	41 04 DD 05 DD 11 DD
	* 	Total length is 21 bytes
	*	MODE01_PID_LEN[] defined the length of each register
	* 	example: MODE01_PID_LEN[0x04] =  1, means 1 bytes
	* 	can_data of multi drive behavior contains 3 parameters,
	* 	so we need 3 cycles process
	*/
	ptr = can_data + 3; //Jump over 41 
	for(index = 0; index < CAN_MULTI_DBH_PID_NUM; index++)
	{
		pid_index = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1)); // get index of pid
		ptr = ptr + 3;
		ptr_data = pid_data; // move pointer of can_data to next byte
		for(pid_cycle = 0; pid_cycle < MODE01_PID_LEN[pid_index]; pid_cycle++)
		{
			*ptr_data = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1));
			ptr  = ptr + 3; // move pointer of can_data to next byte
			ptr_data = ptr_data + 1;
		}
		if(alpha) printf("PID_INDEX[%d]: DATA[0]: %X, DATA[1]: %X\n", pid_index, pid_data[0], pid_data[1]);
		/* 0x04 Calculated engine load value*/
		if( pid_index == 0x04)
		{
			vs->Engine_Load = pid_data[0];
			continue;
		}
		
		/* 0x05 Engine coolant temperature */
		if(pid_index == 0x05)
		{
			vs->Cool_Temp = pid_data[0];
			continue;
		}
		
		/* 0x11 Throttle position */
		if(pid_index == 0x11)
		{
			vs->Pad_Position = pid_data[0];
		}	
	}
	
	return message_length;

}

/* 通过单条CAN指令读取多个OBD寄存器获得使用MAP计算燃油率的相关参数
 * 读取的寄存器包括OB 0F 06 07 0D 0C六个寄存器
 * 截至2014/01/26未验证过
 * 截至2014/03/16 在广汽车上测试，发现数据返回有缺失，所以增加了OBD返回数据长度的判断
 * 如果不是110个字节，则判定为错误数据返回，不做解析
 */ 
int CAN_Multi_MAP(int fd, unsigned char *rxbuf, unsigned char alpha, \
						struct vehicle_status * vs, struct fuel_rate_para * frp)
{
	int ret = 0;
	unsigned char *ptr;
	unsigned char can_data[512];
	int message_length;
	int index;
	int pid_index;
	int pid_cycle;
	unsigned char pid_data[4];
	unsigned char *ptr_data;
	
	memset(can_data, 0x00, 512);
	ret = SerialCommand(fd, "01 0B 0F 06 07 0D 0C\r", 21, rxbuf);
	if(alpha) printf("CAN_MAP RETURN LEN: %d/[%d]\n", ret, CAN_MULTI_MAP_TOTAL);
	if(ret != CAN_MULTI_MAP_TOTAL) return -1;
	/* Process Return Message */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, CAN_MULTI_MAP_LINES);
	if(message_length != CAN_MULTI_MAP_RETURN_DATA) return -1;
	printf("[MAP]OBD>%s\n", rxbuf);
	printf("CAN Data[%d/%d]> %s\n", message_length, CAN_MULTI_MAP_RETURN_DATA, can_data);
	/* Expect Data format:
	*	41 0B DD 0F DD 06 DD 07 DD 0D DD 0C DD DD
	*  Total Length is 42 bytes
	*	MODE01_PID_LEN[] defined the length of each register
	* 	example: MODE01_PID_LEN[0x0B] =  1, means 1 bytes
	* 	can_data of multi map contains 6 parameters,
	* 	so we need 6 [CAN_MULTI_MAP_PID_NUM] cycles process
	*/
	ptr = can_data + 3;  //Jump over 41 
	for(index = 0; index < CAN_MULTI_MAP_PID_NUM; index++)
	{
		pid_index = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1));	// get index of pid
		ptr = ptr + 3; 		// move pointer of can_data to next byte
		ptr_data = pid_data;
		for(pid_cycle = 0; pid_cycle < MODE01_PID_LEN[pid_index]; pid_cycle++)
		{
			*ptr_data = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1));
			ptr  = ptr + 3; // move pointer of can_data to next byte
			ptr_data = ptr_data + 1;
		}
		if(alpha) printf("PID_INDEX[%d]: DATA[0]: %X, DATA[1]: %X\n", pid_index, pid_data[0], pid_data[1]);
		/* 0x0B Intake manifold absolute pressure */
		if( pid_index == 0x0B)
		{
			vs->MAP = pid_data[0];
			frp->map = (int)(vs->MAP);
			continue;
		}
		/* 0x0F Intake air temperature */
		if( pid_index == 0x0F)
		{
			vs->IntakeAirTemp = pid_data[0];
			frp->map = (int)(vs->IntakeAirTemp - 40);
			continue;
		}		
		
		/* 0x06 Short term fuel % Bank 1 */
		if(pid_index == 0x06)
		{
			vs->ECU_STFuelTrim = pid_data[0];
			frp->stft = (int)((vs->ECU_STFuelTrim - 128) * 100 * 100 / 128);
			continue;
		}
		
		/* 0x07 Long term fuel % Bank 1 */
		if(pid_index == 0x07)
		{
			vs->ECU_LTFuelTrim = pid_data[0];
			frp->ltft = (int)((vs->ECU_LTFuelTrim - 128) * 100 * 100/ 128);
			continue;
		}
		/* 0x0D Vehicle speed */
		if(pid_index == 0x0D)
		{
			vs->VehicleSpeed = pid_data[0];
			frp->speed = (int)(vs->VehicleSpeed );
			continue;
		}
		/* 0x0C Engine RPM */
		if(pid_index == 0x0C)
		{
			vs->VehicleEnginRPM[0] = pid_data[0];
			vs->VehicleEnginRPM[1] = pid_data[1];
			frp->rpm = ((vs->VehicleEnginRPM[0]* 256 + vs->VehicleEnginRPM[1]) / 4);
		}	
	}
	
	return message_length;
}


/* 通过单条CAN指令读取多个OBD寄存器获得使用MAF计算燃油率的相关参数
 * 读取的寄存器包括10 06 07 0D 0C五个寄存器
 * 2014/01/26已经在2013款FOCUS车上验证通过
 */ 
int CAN_Multi_MAF(int fd, unsigned char *rxbuf, unsigned char alpha, \
						struct vehicle_status * vs, struct fuel_rate_para * frp)
{
	int ret = 0;
	unsigned char *ptr;
	unsigned char can_data[512];
	int message_length;
	int index;
	int pid_index;
	int pid_cycle;
	unsigned char pid_data[4];
	unsigned char *ptr_data;
	
	
	memset(can_data, 0x00, 512);

	ret = SerialCommand(fd, "01 10 06 07 0D 0C\r", 18, rxbuf);
	printf("[MAF]OBD Retrun Length: %d/[%d]\n", ret, CAN_MULTI_MAF_TOTAL);
	printf("[MAF]OBD>%s\n", rxbuf);
	if(ret != CAN_MULTI_MAF_TOTAL) return -1;
	/* Process Return Messagsr */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, CAN_MULTI_MAF_LINES);	
	printf("CAN Data[%d/%d]> %s\n", message_length, CAN_MULTI_MAF_RETURN_DATA, can_data);
	if(message_length != CAN_MULTI_MAF_RETURN_DATA) return -1;
	
	/* Expect Data format:
	*	41 10 DD DD 06 DD 07 DD 0D DD 0C DD DD 
	* 	Total length is 39 bytes
	*	MODE01_PID_LEN[] defined the length of each register
	* 	example: MODE01_PID_LEN[0x10] =  2, means 2 bytes
	* 	can_data of multi maf contains 5 parameters,
	* 	so we need 5[CAN_MULTI_MAF_PID_NUM] cycles process
	*/
	ptr = can_data + 3;  //Jump over 41 
	for(index = 0; index < CAN_MULTI_MAF_PID_NUM; index++)
	{
		memset(pid_data, 0, 4);
		pid_index = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1)); // get index of pid
		ptr = ptr + 3;
		ptr_data = pid_data; // move pointer of can_data to next byte
		for(pid_cycle = 0; pid_cycle < MODE01_PID_LEN[pid_index]; pid_cycle++)
		{
			*ptr_data = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1));
			ptr  = ptr + 3; // move pointer of can_data to next byte
			ptr_data = ptr_data + 1;
		}
		
		//if(alpha) printf("PID_INDEX[%d]: DATA[0]: %X, DATA[1]: %X\n", pid_index, pid_data[0], pid_data[1]);

		/* 0x0B MAF air flow rate calculation*/
		if( pid_index == 0x10)
		{
			vs->AirFlowRate[0] = pid_data[0];
			vs->AirFlowRate[1] = pid_data[1];
			/*OBD标准结算需要除以100， 此处将结果保留2位有效小数所以没有除以100*/
			frp->maf = (int)((vs->AirFlowRate[0] * 256 + vs->AirFlowRate[1]));
			continue;
		}
		
		/* 0x06 Short term fuel % Bank 1 */
		if(pid_index == 0x06)
		{
			vs->ECU_STFuelTrim = pid_data[0];
			frp->stft = (int)((vs->ECU_STFuelTrim - 128) * 100 * 100 / 128);
			continue;
		}
		
		/* 0x07 Long term fuel % Bank 1 */
		if(pid_index == 0x07)
		{
			vs->ECU_LTFuelTrim = pid_data[0];
			frp->ltft = (int)((vs->ECU_LTFuelTrim - 128) * 100 * 100/ 128);
			continue;
		}
		/* 0x0D Vehicle speed */
		if(pid_index == 0x0D)
		{
			vs->VehicleSpeed = pid_data[0];
			frp->speed = (int)(vs->VehicleSpeed );
			continue;
		}
		/* 0x0C Engine RPM */
		if(pid_index == 0x0C)
		{
			vs->VehicleEnginRPM[0] = pid_data[0];
			vs->VehicleEnginRPM[1] = pid_data[1];
			frp->rpm = ((vs->VehicleEnginRPM[0]* 256 + vs->VehicleEnginRPM[1]) / 4);
		}
		
	}
	return message_length;
	
}

/* 通过单条CAN指令读取多个OBD寄存器获得使用EFR计算燃油率的相关参数
 * 同时读取驾驶行为的3个寄存器
 * 读取的寄存器包括04 05 11 5E 0D 0C六个寄存器
 * 截至2014/01/26未验证过
 */ 
int CAN_Multi_EFR(int fd, unsigned char *rxbuf, unsigned char alpha, \
						struct vehicle_status * vs, struct fuel_rate_para * frp)
{
	int ret = 0;
	unsigned char *ptr;
	unsigned char can_data[512];
	int message_length;
	int index;
	int pid_index;
	int pid_cycle;
	unsigned char pid_data[4];
	unsigned char *ptr_data;
	
	memset(can_data, 0x00, 512);
	ret = SerialCommand(fd, "01 04 05 11 5E 0D 0C\r", 21, rxbuf);
	printf("[ERF]OBD Retrun Length: %d\n", ret);
	/* Process Return Message */	
	message_length = Org_CAN_Message(rxbuf, ret, can_data, CAN_MULTI_MAP_LINES);
	if(alpha) printf("[EFR]OBD>%s\n", rxbuf);
	if(alpha) printf("CAN Data[%d]> %s\n", message_length, can_data);
	if(message_length != CAN_MULTI_EFR_RETURN_DATA) return -1;
	
	/* Expect Data format:
	*	41 04 DD 05 DD 11 DD 5E DD DD 0D DD 0C DD DD
	*  Total Length is 45 bytes
	*	MODE01_PID_LEN[] defined the length of each register
	* 	example: MODE01_PID_LEN[0x04] =  1, means 1 bytes
	* 	can_data of multi map contains 6 parameters,
	* 	so we need 6 [CAN_MULTI_EFR_PID_NUM] cycles process
	*/
	ptr = can_data + 3; // Jump over 41
	for(index = 0; index < CAN_MULTI_EFR_PID_NUM; index++)
	{
		pid_index = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1));	// get index of pid
		ptr = ptr + 3; 		// move pointer of can_data to next byte
		ptr_data = pid_data;
		for(pid_cycle = 0; pid_cycle < MODE01_PID_LEN[pid_index]; pid_cycle++)
		{
			*ptr_data = HexToChar(*ptr) * 16 + HexToChar(*(ptr + 1));
			ptr  = ptr + 3; // move pointer of can_data to next byte
			ptr_data = ptr_data + 1;
		}

		if(alpha) printf("PID_INDEX[%d]: DATA[0]: %X, DATA[1]: %X\n", pid_index, pid_data[0], pid_data[1]);
		
		/* 0x04 Calculated engine load value*/
		if( pid_index == 0x04)
		{
			vs->Engine_Load = pid_data[0];
			continue;
		}
		
		/* 0x05 Engine coolant temperature */
		if(pid_index == 0x05)
		{
			vs->Cool_Temp = pid_data[0];
			continue;
		}
		
		/* 0x11 Throttle position */
		if(pid_index == 0x11)
		{
			vs->Pad_Position = pid_data[0];
			continue;
		}

		/* 0x5E Engine fuel rate */
		if(pid_index == 0x5E)
		{
			vs->EngineFuelRate[0] = pid_data[0];
			vs->EngineFuelRate[1] = pid_data[1];
			//结果放大100倍，保留2位有效小数
			frp->fuelrate = (int)((vs->EngineFuelRate[0]* 256 + vs->EngineFuelRate[1]) * 5);
			continue;
		}
		
		/* 0x0D Vehicle speed */
		if(pid_index == 0x0D)
		{
			vs->VehicleSpeed = pid_data[0];
			frp->speed = (int)(vs->VehicleSpeed );
			continue;
		}
		/* 0x0C Engine RPM */
		if(pid_index == 0x0C)
		{
			vs->VehicleEnginRPM[0] = pid_data[0];
			vs->VehicleEnginRPM[1] = pid_data[1];
			frp->rpm = ((vs->VehicleEnginRPM[0]* 256 + vs->VehicleEnginRPM[1]) / 4);
		}	
	}
	
	return message_length;
}


/*	通过MAP来计算燃油率
 * 2014/01/26 已经验证与设计文档结果一致
 * 计算结果保留4为有效小数
 *	计算结果单位为  ml/s
 */ 
int Cal_FR_From_MAP(struct fuel_rate_para *frp)
{

	unsigned long ret[7] = {0,0,0,0,0,0,0};
	unsigned int maf_integer = 0;
	unsigned int maf_decimal = 0;
	int index;
	
	//通过MAP计算MAF
	//MAF = RPM * VE * MAP * ED * MAIR / 2 * 60 * R *IAT
	// VE = 0.7
	//	ED = 2.4	发动机排量
	// MAIR = 28.97
	// R = 8.314
	// 将公式中的常量独立计算   VE*ED*MAIR/120 * R = 0.0487827
    // MAF = （RPM * MAP / IAT) * 0.0487827

	ret[0] = frp->rpm * frp->map * 100;	// 将分子放大100倍，保留2位小数
	ret[0] /= (frp->iat + 273);
	
	//移位相乘 4.87827， 结果整数中有4位小数
	maf_integer = 0;
	for(index = 0; index < 6; index++){
		ret[index+1] = ret[0] * map_para1[0][index] / map_para2[index];
		maf_integer+=ret[index+1];
	}
	//结果缩小100，结果中仍只保留2位小数
	frp->maf = maf_integer / 100;
	//原结果中小数第三位四舍五入  
	maf_decimal = maf_integer % 100;
	if(maf_decimal >= 50 ) frp->maf += 1;
	
	//由MAF计算燃油率
	//公式： Fuel_rate(ml/s) = MAF/ AFR * P
	// AFR = 14.64
	//  P = 0.74
	//公式转变 ： Fuel_rate(ml/s) = MAF * 0.0923
	ret[0] = frp->maf *  maf_para;		//maf_para = 923， 结果放大10000倍
	
	//精度缩小100倍并四舍五入，整数结果中有4位小数
	maf_decimal = ret[0] % 100;
	ret[0] /= 100;
	if(maf_decimal >= 50) ret[0]++;
	
	//短期燃油修正
	ret[0] = ret[0] * (10000 + frp->stft) / 10000;

	//长期燃油修正
	ret[0] = ret[0] * (10000 + frp->ltft) / 10000;
	
	//最终结果整数中含4位小数
	frp->fuelrate = (int) (ret[0]); // ml/s
	
	//修改日期： 2013/10/08
	//修改人： ray goo
	//修改原因： 由于在分钟时间切片的时候会计算平均百公里油耗，所以在秒级的运算过程中不进行计算
	/*
	if(frp->speed > 0 ){
		frp->fuelrate100 = (frp->fuelrate * 1360)/ frp->speed;
		maf_decimal = frp->fuelrate100 % 100;
		frp->fuelrate100 /= 100;
		if(maf_decimal >= 50) frp->fuelrate100++;
	}
	else
		frp->fuelrate100 = 5000;	
	*/
	return frp->fuelrate;
}


/* 通过 MAF 计算燃油率
 * 2014/01/26 已经验证与设计文档结果一致
 * 计算结果保留4为有效小数
 *	计算结果单位为  ml/s
 */
int Cal_FR_From_MAF(struct fuel_rate_para *frp)
{
	//由MAF计算燃油率
	//公式： Fuel_rate(ml/s) = MAF/ （AFR * P）
	// AFR = 14.64
	//  P = 0.74
	//公式转变 ： Fuel_rate(ml/s) = MAF * 0.0923
	unsigned long ret;
	unsigned int maf_decimal = 0;
	
	ret = frp->maf *  maf_para;		//maf_para = 923， 结果放大10000倍

	//精度缩小100倍并四舍五入，整数结果中有4位小数
	maf_decimal = ret % 100;
	ret /= 100;
	if(maf_decimal >= 50) ret++;
	
	//短期燃油修正
	ret = ret * (10000 + frp->stft) / 10000;

	//长期燃油修正
	ret = ret * (10000 + frp->ltft) / 10000;
	
	//最终结果整数中含4位小数, 单位为mL/s
	frp->fuelrate = (int) (ret); 
	return frp->fuelrate;
}

/*	通过EFR 计算燃油率
 *	截至2014/01/26 未通过实验验证
 */ 
int Cal_FR_From_EFR(struct fuel_rate_para *frp)
{
	/* EFR 输出的结果放大了100倍数，即OBD输出值EFR * 100， 保留2位有效小数
	 *	OBD输出结果的单位为 L/H
	 *	要转换成系统统一计算单位 mL/s，结果需要 乘1000 除以 3600秒
	 * 保留4位有效小数，所以需要再放大100倍
	 */ 
	frp->fuelrate = frp->fuelrate * 1000 / 36;
	return frp->fuelrate;
}


